# -*- encoding: utf-8 -*-
"""
@File    :   walkToObstacle.py    

@Contact :   2055466817@qq.com

@Modify Time :   2020/12/2 上午8:05 

@Author :   赵方国        
"""
import almath
from naoqi import ALProxy
import cv2 as cv
import random
import time
from ImagProcess import getImag
from obstacle import correctAngle


def walkToObstacleTOWARDversion(robotIP, PORT, angleINIT, flag):
    """
    采用moveToward函数接近障碍物，同时使用距离来判断
    @param robotIP:
    @param PORT:
    @param angleINIT:初始角度
    @param flag: 障碍物的颜色，字符串
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    correctAngle(robotIP, PORT, angleINIT)
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    name = str(random.randint(0,1000))
    data = getImag(robotIP, 9559, 1, flag, name)  # 使用下摄像头
    h = data[2]
    w = data[3]
    while h <= 140 or w <= 175:  # TODO 此处为细调阈值 y+h > 120
        # motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
        motionProxy.moveToward(0.4, 0, 0, lower_steps)
        name = str(random.randint(0, 1000))
        data = getImag(robotIP, 9559, 1, flag, name)
        # y = data[1]
        x = data[0]
        h = data[2]
        w = data[3]
        center = x + h / 2
        print h, center
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove = robotMove[0]
        if robotMove > 0.15:
            correctAngle(robotIP, PORT, angleINIT)
            initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        if center < 120:
            motionProxy.moveTo(0, 0.03, 0, smallTurnStep)
        if center > 200:
            motionProxy.moveTo(0, -0.03, 0, smallTurnStep)
    tts.say('finish')
    motionProxy.stopMove()
    correctAngle(robotIP, PORT, angleINIT)
    time.sleep(1)


def walkToObstacle(robotIP, PORT, angleINIT, flag):
    """
    函数执行走到障碍物的前方，使用moveTo的方式，未使用

    @param robotIP:
    @param PORT:
    @param angleINIT:初始角度值
    @param flag:面前的障碍物的颜色，有“blue”,"red","yellow"三种
    @return:
    """
    # TODO 此处的步态参数需要更改
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)

    correctAngle(robotIP, PORT, angleINIT)

    width = 100
    height = 100
    # data = getImag(robotIP, 9559, 0, flag)  # 使用上摄像头
    i = 0
    # while (width * height) <= 18000:
    #     name = str(random.randint(0, 1000))
    #     data = getImag(robotIP, 9559, 0, flag, name)
    #     width = data[2]
    #     height = data[3]
    #     print width * height
    #     motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    #     i += 1
    #     if i % 2 == 0:
    #         angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    #         print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    #         if abs(angle - angleINIT) > 5 * pi / 180:
    #             motionProxy.moveTo(0, 0, -(angle - angleINIT))
    #             print 'turn'
    # tts.say("turn to low camera")
    # motionProxy.moveTo(0.35, 0, 0)
    correctAngle(robotIP, PORT, angleINIT)
    data = getImag(robotIP, 9559, 1, flag)  # 使用下摄像头
    y = data[1]
    h = data[2]
    w = data[3]
    i = 0
    while h <= 130 or w <= 165:  # TODO 此处为细调阈值 y+h > 120
        name = str(random.randint(0, 1000))
        data = getImag(robotIP, 9559, 1, flag, name)
        # y = data[1]
        h = data[2]
        w = data[3]
        print h
        motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
        # i += 1
        # if i % 2 == 0:
        correctAngle(robotIP, PORT, angleINIT)
    if h > 190 or w > 220:  # TODO 此处应该还需要更改
        motionProxy.moveTo(-0.05, 0, 0, smallTurnStep)
        walkToObstacle(robotIP, PORT, angleINIT, flag)
    tts.say('finish')


def alignToObstacle(robotIP, PORT, flag, angleINIT):
    """
    对齐障碍物，进行左右平移，有校准

    :param robotIP:
    :param PORT:
    :param flag:传入的是面前的障碍物的颜色，为字符串类型
    :return:
    """
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    name = str(random.randint(0, 1000))
    data = getImag(robotIP, PORT, 1, flag, name)  # TODO 此处默认初始对齐操作时的障碍物应该为蓝色
    center = data[0] + data[2] / 2
    print "center is :"
    print center
    if 170 > center > 150:  # TODO 此处的范围有问题，需要更改
        # time.sleep(1)
        angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
        print "center"
        return angle
    if center < 150:
        motionProxy.moveTo(0, 0.05, 0)
        print "move TO LEFT"
        # correctAngle(robotIP, PORT, angleINIT)
        alignToObstacle(robotIP, PORT, flag, angleINIT)
    if center > 170:
        motionProxy.moveTo(0, -0.05, 0)
        # correctAngle(robotIP, PORT, angleINIT)
        print "move TO RIGHT"
        alignToObstacle(robotIP, PORT, flag, angleINIT)


def walkToEnd(robotIP, PORT, angleINIT):
    """
    冲线函数，执行到机器人视野中没有蓝色的终点线时停止,使用moveToward，有校准
    @param robotIP:
    @param PORT:
    @param angleINIT: 初始角度，用于校准
    @return:
    """
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    name = random.randint(0, 1000)  # 没有物体时会输出【0， 0， 0， 0】
    data = [100, 100, 100, 100]
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))

    while True:
        name = str(random.randint(0, 1000))
        data = getImag(robotIP, PORT, 1, "blue", name)
        motionProxy.moveToward(0.4, 0, 0, lower_steps)
        # correctAngle(robotIP, PORT, angleINIT)
        if data[0] + data[1] + data[2] + data[3] == 0:
            break
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove = robotMove[0]
        if robotMove > 0.2:
            correctAngle(robotIP, PORT, angleINIT)
            initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)

    motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    return True


def lineCorrect(robotIP, PORT, angleINIT):
    """
    白线校准函数，用于辅助判断是否在避障路径中超过场线,没有使用
    @param robotIP:
    @param PORT:
    @param angleINIT: 角度初值用于校准
    @return:
    """
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)

    angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    name = str(random.randint(0, 1000))
    data = getImag(robotIP, PORT, 1, name, "white")
    y = data[1]
    w = data[3]
    if y + w < 200:
        while (y + w) < 200:
            motionProxy.moveToward(0.1, 0, 0, lower_steps)
            data = getImag(robotIP, PORT, 1, name, "white")
            y = data[1]
            w = data[2]
            print (y + w)
            correctAngle(robotIP, PORT, angleINIT)
        tts.say('stop and stop')
        motionProxy.stopMove()
        time.sleep(1)
        return True
    else:
        return True


if __name__ == '__main__':
    robotIP = "169.254.90.118"
    PORT = 9559
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)

    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    angleINTI = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # angleIMIT = alignToObstacle(robotIP, PORT)
    # print "angle INIT"
    # print angleIMIT
    # walkToObstacle(robotIP, PORT, angleINTI, "blue")
    walkToObstacleTOWARDversion(robotIP, PORT, angleINTI, "blue")
    # walkToEnd(robotIP, PORT, angleINTI)
    # alignToObstacle(robotIP, PORT, "yellow")
    motionProxy.rest()
    cv.waitKey(0)
